{
  "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9711476a",
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy\n",
    "import IPython.display as disp"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c3000c03",
   "metadata": {},
   "outputs": [],
   "source": [
    "import helper_functions as hf"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "55a00e85",
   "metadata": {},
   "outputs": [],
   "source": [
    "sympy.init_printing()\n",
    "\n",
    "dt = sympy.Symbol(\"\\Delta t\", real=True)\n",
    "g = sympy.Symbol(\"g\", real=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "757a504c",
   "metadata": {},
   "source": [
    "**Integrated gyroscope measurements**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f4f51c5f",
   "metadata": {},
   "outputs": [],
   "source": [
    "delta_angle_x = sympy.Symbol(\"\\Delta \\Omega_x\", real=True)\n",
    "delta_angle_y = sympy.Symbol(\"\\Delta \\Omega_y\", real=True)\n",
    "delta_angle_z = sympy.Symbol(\"\\Delta \\Omega_z\", real=True)\n",
    "delta_angle = sympy.Matrix([delta_angle_x, delta_angle_y, delta_angle_z])\n",
    "delta_angle"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1a362be8",
   "metadata": {},
   "source": [
    "**Integrated accelerometer measurements**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c88bd90f",
   "metadata": {},
   "outputs": [],
   "source": [
    "delta_velocity_x = sympy.Symbol(\"\\Delta v_x\", real=True)\n",
    "delta_velocity_y = sympy.Symbol(\"\\Delta v_y\", real=True)\n",
    "delta_velocity_z = sympy.Symbol(\"\\Delta v_z\", real=True)\n",
    "delta_velocity = sympy.Matrix([delta_velocity_x, delta_velocity_y, delta_velocity_z])\n",
    "delta_velocity"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f17a14ea",
   "metadata": {},
   "source": [
    "**Combined IMU measurements**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e7b77e61",
   "metadata": {},
   "outputs": [],
   "source": [
    "u = sympy.Matrix([delta_angle, delta_velocity])\n",
    "u"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "87fae3ac",
   "metadata": {},
   "source": [
    "**Gyroscope noise variance**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "edf2082b",
   "metadata": {},
   "outputs": [],
   "source": [
    "delta_angle_x_var = sympy.Symbol(\"\\sigma^2_{\\Delta \\Omega_x}\", real=True)\n",
    "delta_angle_y_var = sympy.Symbol(\"\\sigma^2_{\\Delta \\Omega_y}\", real=True)\n",
    "delta_angle_z_var = sympy.Symbol(\"\\sigma^2_{\\Delta \\Omega_z}\", real=True)\n",
    "delta_angle_var = sympy.Matrix([delta_angle_x_var, delta_angle_y_var, delta_angle_z_var])\n",
    "delta_angle_var"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8eb758ae",
   "metadata": {},
   "source": [
    "**Accelerometer noise variance**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "517d30e6",
   "metadata": {},
   "outputs": [],
   "source": [
    "delta_velocity_x_var = sympy.Symbol(\"\\sigma^2_{\\Delta v_x}\", real=True)\n",
    "delta_velocity_y_var = sympy.Symbol(\"\\sigma^2_{\\Delta v_y}\", real=True)\n",
    "delta_velocity_z_var = sympy.Symbol(\"\\sigma^2_{\\Delta v_z}\", real=True)\n",
    "delta_velocity_var = sympy.Matrix([delta_velocity_x_var, delta_velocity_y_var, delta_velocity_z_var])\n",
    "delta_velocity_var"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3af8cb66",
   "metadata": {},
   "source": [
    "**Combined IMU noise variance**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0d599af4",
   "metadata": {},
   "outputs": [],
   "source": [
    "u_var = sympy.Matrix.diag(delta_angle_x_var, delta_angle_y_var, delta_angle_z_var, delta_velocity_x_var, delta_velocity_y_var, delta_velocity_z_var)\n",
    "u_var"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f660f5ba",
   "metadata": {},
   "source": [
    "## Definition of state vector\n",
    "### orientation\n",
    "Using w-x-y-z convention"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "de516062",
   "metadata": {},
   "outputs": [],
   "source": [
    "qw = sympy.Symbol(\"q_w\", real=True)\n",
    "qx = sympy.Symbol(\"q_x\", real=True)\n",
    "qy = sympy.Symbol(\"q_y\", real=True)\n",
    "qz = sympy.Symbol(\"q_z\", real=True)\n",
    "q = sympy.Matrix([qw, qx, qy, qz])\n",
    "q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4b0cef52",
   "metadata": {},
   "outputs": [],
   "source": [
    "R_to_earth = hf.quat2Rot(q)\n",
    "R_to_body = R_to_earth.T\n",
    "R_to_earth"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "75927d0f",
   "metadata": {},
   "source": [
    "### velocity\n",
    "Using the map (ENU) reference frame, following the ROS convention."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4b0cd339",
   "metadata": {},
   "outputs": [],
   "source": [
    "vx = sympy.Symbol(\"v^{ENU}_x\", real=True)\n",
    "vy = sympy.Symbol(\"v^{ENU}_y\", real=True)\n",
    "vz = sympy.Symbol(\"v^{ENU}_z\", real=True)\n",
    "v = sympy.Matrix([vx, vy, vz])\n",
    "v"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f4843f9c",
   "metadata": {},
   "source": [
    "### Position\n",
    "Using the map (ENU) reference frame, following the ROS convention."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a55f0c3f",
   "metadata": {},
   "outputs": [],
   "source": [
    "px = sympy.Symbol(\"p^{ENU}_x\", real=True)\n",
    "py = sympy.Symbol(\"p^{ENU}_y\", real=True)\n",
    "pz = sympy.Symbol(\"p^{ENU}_z\", real=True)\n",
    "p = sympy.Matrix([px, py, pz])\n",
    "p"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4a512813",
   "metadata": {},
   "source": [
    "### Delta angle bias"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ce71f973",
   "metadata": {},
   "outputs": [],
   "source": [
    "delta_angle_bias_x = sympy.Symbol(\"\\Delta \\Omega_{bias,x}\", real=True)\n",
    "delta_angle_bias_y = sympy.Symbol(\"\\Delta \\Omega_{bias,y}\", real=True)\n",
    "delta_angle_bias_z = sympy.Symbol(\"\\Delta \\Omega_{bias,z}\", real=True)\n",
    "delta_angle_bias = sympy.Matrix([delta_angle_bias_x, delta_angle_bias_y, delta_angle_bias_z])\n",
    "delta_angle_true = delta_angle - delta_angle_bias\n",
    "delta_angle_true"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "eae69044",
   "metadata": {},
   "source": [
    "### Delta velocity bias"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e25026d6",
   "metadata": {},
   "outputs": [],
   "source": [
    "delta_velocity_bias_x = sympy.Symbol(\"\\Delta v_{bias, x}\", real=True)\n",
    "delta_velocity_bias_y = sympy.Symbol(\"\\Delta v_{bias, y}\", real=True)\n",
    "delta_velocity_bias_z = sympy.Symbol(\"\\Delta v_{bias, z}\", real=True)\n",
    "delta_velocity_bias = sympy.Matrix([delta_velocity_bias_x, delta_velocity_bias_y, delta_velocity_bias_z])\n",
    "delta_velocity_true = delta_velocity - delta_velocity_bias\n",
    "delta_velocity_true"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2a28ef85",
   "metadata": {},
   "source": [
    "### State vector"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "adaccd02",
   "metadata": {},
   "outputs": [],
   "source": [
    "state = sympy.Matrix([q, v, p, delta_angle_bias, delta_velocity_bias])\n",
    "state"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "12a49416",
   "metadata": {},
   "source": [
    "## State propagation/prediction\n",
    "### Orientation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "11f83ee1",
   "metadata": {},
   "outputs": [],
   "source": [
    "q_new = hf.quat_mult(q, sympy.Matrix([1, 0.5 * delta_angle_true[0], 0.5 * delta_angle_true[1], 0.5 * delta_angle_true[2]]))\n",
    "q_new"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6ef73594",
   "metadata": {},
   "source": [
    "### Velocity\n",
    "subtract gravity vector (not sure about sign)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "564722f8",
   "metadata": {},
   "outputs": [],
   "source": [
    "v_new = v + R_to_earth * delta_velocity_true + sympy.Matrix([0, 0, -g]) * dt\n",
    "v_new"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "923a5a24",
   "metadata": {},
   "source": [
    "### Position"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1f06e9f0",
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "p_new = p + v * dt\n",
    "p_new"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b78aa84a",
   "metadata": {},
   "source": [
    "### IMU bias"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5fb05c89",
   "metadata": {},
   "outputs": [],
   "source": [
    "delta_angle_bias_new = delta_angle_bias\n",
    "delta_velocity_bias_new = delta_velocity_bias"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "251c2e6e",
   "metadata": {},
   "source": [
    "### State"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "af3f3f1a",
   "metadata": {},
   "outputs": [],
   "source": [
    "state_new = sympy.Matrix([q_new, v_new, p_new, delta_angle_bias_new, delta_velocity_bias_new])\n",
    "state_new"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f1c1e011",
   "metadata": {},
   "source": [
    "## State propagation/prediction jacobians"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2ae971fa",
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "F = state_new.jacobian(state)\n",
    "F"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6fc7e0f8",
   "metadata": {},
   "source": [
    "### Process Noise"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d6728959",
   "metadata": {},
   "outputs": [],
   "source": [
    "G = state_new.jacobian(u)\n",
    "G"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0931f812",
   "metadata": {},
   "outputs": [],
   "source": [
    "Q = G * u_var * G.T\n",
    "Q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "93486502",
   "metadata": {},
   "outputs": [],
   "source": [
    "P = hf.create_symmetric_cov_matrix([sympy.shape(state)[0], sympy.shape(state)[0]])\n",
    "# P"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a1fc525d",
   "metadata": {},
   "outputs": [],
   "source": [
    "tmp1 = F * P"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4775d80c",
   "metadata": {},
   "outputs": [],
   "source": [
    "tmp2 = tmp1 * F.T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0f0866e7",
   "metadata": {},
   "outputs": [],
   "source": [
    "P_new = tmp2 + Q\n",
    "for row in range(sympy.shape(P_new)[0]):\n",
    "    for col in range(sympy.shape(P_new)[1]):\n",
    "        if row > col:\n",
    "            P_new[row, col] = 0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bece2730",
   "metadata": {},
   "outputs": [],
   "source": [
    "P_new_simple = sympy.cse(P_new, sympy.utilities.iterables.numbered_symbols(prefix='tmp'), optimizations='basic')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e27e14b5",
   "metadata": {},
   "outputs": [],
   "source": [
    "with open(\"test.c\", \"w\") as f:\n",
    "    hf.write_subexpressions(f, P_new_simple[0])\n",
    "    hf.write_matrix(f, sympy.Matrix(P_new_simple[1]), \"P_new\", True)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "df544972",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
